function [ dDupsilonBl, DupsilonTildeSl ] = accerror_inc( DupsilonBl, accPar, Tl, DalphaBl, DalphaBl1, ignore2Od ) %#codegen
%ACCERROR_INC 计算加速度计器件误差引入的测量误差
%
% Tips
% # 本函数针对增量形式
%
% Input Arguments
% # DupsilonBl: 3*n矩阵，n为采样个数，比速度增量，单位m/s
% # Tl: 标量，采样周期，单位s
% # accPar: 各域为数组的结构体，加速度计的确定性误差参数，各域最后一维为时间m
%       accPar.KScal0: 3*n矩阵，加速度计标度因数标称值，单位：1m/s对应的脉冲数
%       accPar.PS0B: 3*3*n数组，传感器系与本体系之间的坐标转换矩阵标称值，无单位，默认全为单位阵
%       accPar.dfBiasS: 3*n矩阵，加速度计零偏，单位：m/s^2
%       accPar.dKScalP: 3*x*n数组，x为考虑的标度因数误差的阶次，x=1，模型为线性，加速度计正向标度因数误差，单位与阶次相关，第1列无单位，第i列单位(m/s^2)^-(i-1)，默认全为0
%       accPar.dKScalN: 3*x*n数组，x为考虑的标度因数误差的阶次，x=1，模型为线性，加速度计负向标度因数误差，单位与阶次相关，第1列无单位，第i列单位(m/s^2)^-(i-1),默认全为0
%       accPar.dPSS0: 3*3*n数组，失准角（安装误差），无单位，默认全为0
%       accPar.dfQuantS: 3*n矩阵，加速度计量化误差，单位：m/s^2，默认全为0
%       accPar.lB: 3*3*n数组，按列分别为3只加速度计的杆臂项，单位：m，默认全为0
%       accPar.KAniso: 3*n矩阵，3只加速度计不等惯量误差的系数，单位：(m/s^2)/(rad/s)^2，默认全为0
%       accPar.CBA: 3*3*3*n数组，按页分别为本体系到3只加速度计坐标系的坐标转换矩阵，无单位，默认各页全为单位阵
% # DalphaBl: 3*n矩阵，角增量，单位rad
% # DalphaBl1: 3*1向量，DalphaBl矩阵首周期前一周期的角增量（用于连续采样）；或3*n矩阵，DalphaBl矩阵各周期前一周期的角增量（用于不连续采样），单位rad
% # ignore2Od: 逻辑标量，true表示忽略2阶误差，默认为true
%
% Output Arguments
% # dDupsilonBl: 3*n矩阵，各采样时刻的比速度增量误差，单位m/s
% # DupsilonTildeSl: 3*n矩阵，各采样时刻S系下的比速度增量（包含测量误差），单位m/s
%
% References
% # 《应用导航算法工程基础》“惯性传感器模型” 总误差模型

if nargin < 6
    ignore2Od = true;
end
n = size(DupsilonBl, 2);

dDupsilonBl = coder.nullcopy(NaN(3, n));
if nargout > 1
    DupsilonTildeSl = coder.nullcopy(NaN(3, n));
end
for j=1:n
    %% 计算加速度计各项误差
    if size(DalphaBl1, 2) == 1
        if j == 1
            DalphaBl1_effective = DalphaBl1;
        else
            DalphaBl1_effective = DalphaBl(:, j-1);
        end
    else % 传入所有周期的前点值，在测试用例中使用
        DalphaBl1_effective = DalphaBl1(:, j);
    end
    % 杆臂效应误差-比速度增量形式
    PSBT = (accPar.PS0B(:, :, j)*(eye(3)+accPar.dPSS0(:, :, j)))';
    dfSizeSInt = acclevarmerr_inc(PSBT, accPar.lB(:, :, j), DalphaBl(:, j), DalphaBl1_effective, Tl);
    % 不等惯量误差-比速度增量形式
    dfAnisoSInt = accanisoerr_inc(accPar.KAniso(:, j), accPar.CBA(:, :, :, j), DalphaBl(:, j), DalphaBl1_effective, Tl);
    %% 计算加速度计比速度增量输出值dupsilonTildeS与比速度增量理论值dupsilon之差
    % 不加标度因数误差阵的比速度增量输出
    dfBiasExSInt = accPar.dfBiasS(:, j)*Tl + dfSizeSInt + dfAnisoSInt + accPar.dfQuantS(:, j)*Tl;
    DupsilonSl = PSBT * DupsilonBl(:, j);
    DupsilonTildeScalSl = DupsilonSl + accPar.dfBiasS(:, j)*Tl + dfSizeSInt + dfAnisoSInt;
    % 计算j时刻标度因数误差阵
    accdKScalCoef = scalfacterrsign(accPar.dKScalP(:, :, j), accPar.dKScalN(:, :, j), DupsilonTildeScalSl);
    % 计算dKScal
    dKScal = polyscalfact(DupsilonTildeScalSl, accdKScalCoef, ones(3, 1), Tl); % NOTE: vTildeScalS计算式应与fgjacobian及imuout2in函数一致
    % 忽略二阶误差的加速度计误差
    dPSBT = (accPar.PS0B(:, :, j)*accPar.dPSS0(:, :, j))';
    dDupsilonBl(:, j) = (accPar.PS0B(:, :, j)')\(dKScal.*(accPar.PS0B(:, :, j)'*DupsilonBl(:, j))+dPSBT*DupsilonBl(:, j)+dfBiasExSInt);
    if ~ignore2Od
        % 考虑二阶误差的加速度计误差
        dDupsilonBl2 = (accPar.PS0B(:, :, j)')\(dKScal.*(dPSBT*DupsilonBl(:, j)+dfBiasExSInt));
        dDupsilonBl(:, j) = dDupsilonBl(:, j)+dDupsilonBl2;
    end
    if nargout > 1
        DupsilonTildeSl(:, j) = (ones(3, 1)+dKScal) .* (DupsilonSl+dfBiasExSInt);
    end
end
end